/**
 * @file:          TrajectoryStepGenerator.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.11.01
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.11.01,17:52:11
 */

/* Include Files **************************************************************/
#include "TrajectoryStepGenerator.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void TrajectoryStepGenerator_Init(struct TrajectoryStepGenerator *self)
{
    self->Run = TrajectoryStepGenerator_Run;
}

float TrajectoryStepGenerator_Run(struct TrajectoryStepGenerator *self)
{
    float out = 0;

    return out;
}

float TrajectoryStepGenerator_GenerateTrajectory(struct TrajectoryStepGenerator *self)
{
    self->T[0] = self->param_max_speed / self->param_max_acc;
    self->v[0] = self->param_max_acc * self->T[0];
    self->s[0] = self->v[0] * self->T[0];
    self->s[2] = self->s[0];
    self->displacement = self->in_pos_target - self->in_pos_origin;

    if (self->displacement <= (self->s[0] + self->s[2]))
    {
        self->mode = 0;

        /*  */
        
    }
    else
    {
        self->mode = 1;

        /*  */
        self->T[1] = (self->displacement - self->s[1] - self->s[0]) / self->param_max_speed;
        self->T[2] = self->T[0];

        self->t[0] = 0;
        self->t[1] = self->t[0] + self->T[0];
        self->t[2] = self->t[1] + self->T[1];
        self->t[3] = self->t[2] + self->T[2];
    }



    self->T[2] = self->T[0];

    self->t[0] = 0;
}

float TrajectoryStepGenerator_GenerateStep(struct TrajectoryStepGenerator *self)
{
}